#include "StdAfx.h"
#include "DepthFirstRouter.h"

using namespace std;

tPos::tPos()
{
	x = y = -1;
}

void tPos::assign(int x0, int y0)
	{
		x = x0;
		y = y0;
	}
void tPos::reset()
	{
		x = y = -1;
	}
bool tPos::operator==(const tPos &v)
	{
		if (v.x==x && v.y ==y)
		{
			return true;
		} 
		else
		{
			return false;
		}		
	}

bool tPos::operator!=(const tPos &v)
	{
		return ((x!=v.x)||(y!=v.y));
	}

tNode::tNode()
	{
		state.assign(-1, -1);
		parent.assign(-1, -1);
		action = 's';
		cost = 0;
		t = 0;
		isSolution = false;
	}

void tNode::reset()
	{
		state.assign(-1, -1);
		parent.assign(-1, -1);
		action = 's';
		cost = 0;
		t = 0;
		isSolution = false;
	}

bool tNode::operator==(const tNode &v)
	{
		if (state==v.state && t ==v.t)
		{
			return true;
		} 
		else
		{
			return false;
		}		
	}

bool tNode::operator!=(const tNode &v)
	{
		return ((state!=v.state)||(t!=v.t));
	}

bool operator<(const tNode &n1, const tNode &n2)
{	return n1.cost<n2.cost;  };
bool operator>(const tNode &n1, const tNode &n2)
{	return n1.cost>n2.cost;  };


DepthFirstRouter::DepthFirstRouter(void)
{
}

DepthFirstRouter::~DepthFirstRouter(void)
{
}

std::vector<iPos> DepthFirstRouter::route(std::vector< std::vector< char >> map, fPos nowPos, iPos targetPos)
{
	int i;

	vector<iPos> path;
	iPos inode;

	tNode node, next, paNode, startPNode;
	tPos state, posC;
	multimap<int, tNode>::iterator		itr;
	vector<tNode> expdNodes;
	string rt;
	int maxT=999, j, w=50, t=0;
	unsigned int bDown=true, bDownPre=bDown;
	double cost;

	// read the map data
	
	expdNodes.clear();
	w=50; t=0;
	bDown=true; bDownPre=bDown;
	while(OpenSet.size()>0)
		OpenSet.pop();
	ClosedSet.clear();
	TrackPosC.clear();
	StartC.reset(); StartP.reset(); EndC.reset();
	node.reset(); next.reset(); paNode.reset(); startPNode.reset();
	state.reset(); posC.reset();

	MapH = map.size();
	MapW = map[0].size();
	

	if (map[targetPos.y][targetPos.x]=='1')
	{
		// the target is illegal
		return path;
	}
	//////////////////////////////////////////////////////////////////////////
	// - find the solution by uniform cost search
	//////////////////////////////////////////////////////////////////////////
	StartP.assign(nowPos.x, nowPos.y);
	node.state = StartP;
	node.isSolution = false;
	OpenSet.push(node);

	// search loop
	while (1)
	{
		if(OpenSet.empty())	
			break;

		node = OpenSet.top();
		OpenSet.pop();

		// if catch the criminals save the result
		if (node.isSolution)
		{
			inode.x = node.state.x;
			inode.y = node.state.y;
			path.push_back(inode);
			cost = node.cost;
			paNode.state = node.parent;
			paNode.t=node.t-1;
			rt = node.action;
			startPNode.state = StartP;
			startPNode.t = 0;
			if (node.parent.x>-1 && node.parent.y>-1)
			{
				while ((itr=find_state_in_closed(paNode))->second!=startPNode)
				{
					rt += itr->second.action;

					inode.x = itr->second.state.x;
					inode.y = itr->second.state.y;
					path.push_back(inode);

					paNode.state = itr->second.parent;
					paNode.t--;

				}
				inode.x = startPNode.state.x;
				inode.y = startPNode.state.y;
				path.push_back(inode);
			}				
			//rt = revString(rt);
			reverse(path.begin(), path.end());
			break;
		}

		// if not, prepare for continuing searching
		if(find_state_in_closed(node)==ClosedSet.end())
		{
			ClosedSet.insert(pair<int, tNode>(node.state.x, node));
			if (node.t+1<maxT)
			{
				// try to expand the right node
				next = node;
				next.state.x++;
				next.t++;
				next.isSolution = false;
				if (next.state.x<MapW && map[next.state.y][next.state.x]!='1' )
				{
					next.action = 'r';
					next.cost++; 				
					next.parent = node.state;

					if (next.state.x == targetPos.x && next.state.y == targetPos.y)
					{
						next.isSolution = true;
					}

					OpenSet.push(next);
				}
				// try to expand the down node
				next = node;
				next.state.y++;
				next.t++;
				next.isSolution = false;
				if (next.state.y<MapH && map[next.state.y][next.state.x]!='1')
				{
					next.action = 'd';
					next.cost++; 				
					next.parent = node.state;

					if (next.state.x == targetPos.x && next.state.y == targetPos.y)
					{
						next.isSolution = true;
					}

					OpenSet.push(next);
				}
				// try to expand the left node
				next = node;
				next.state.x--;
				next.t++;
				next.isSolution = false;
				if (next.state.x>=0 && map[next.state.y][next.state.x]!='1')
				{
					next.action = 'l';
					next.cost++; 				
					next.parent = node.state;

					if (next.state.x == targetPos.x && next.state.y == targetPos.y)
					{
						next.isSolution = true;
					}

					OpenSet.push(next);
				}
				// try to expand the up node
				next = node;
				next.state.y--;
				next.t++;
				next.isSolution = false;
				if (next.state.y>=0 && map[next.state.y][next.state.x]!='1')
				{
					next.action = 'u';
					next.cost++; 				
					next.parent = node.state;

					if (next.state.x == targetPos.x && next.state.y == targetPos.y)
					{
						next.isSolution = true;
					}

					OpenSet.push(next);
				}	
				
			}

		}
	}

	return path;
}

